//-*- mode: c++ -*-
#ifndef _LIL_MATRIX_SYM_AMD_H_
#define _LIL_MATRIX_SYM_AMD_H_

//adapted from EIGEN
//need to include license file later

namespace amd {
	inline int amd_flip(const int& i) { return -i-2; }

	/* clear w */
	inline int wclear (int mark, int lemax, int *w, int n)
	{
		int k;
		if(mark < 2 || (mark + lemax < 0))
		{
			for(k = 0; k < n; k++)
			if(w[k] != 0)
			w[k] = 1;
			mark = 2;
		}
		return (mark);     /* at this point, w[0..n-1] < mark holds */
	}

	/* depth-first search and postorder of a tree rooted at node j */
	inline int tdfs(int j, int k, int *head, const int *next, int *post, int *stack)
	{
		int i, p, top = 0;
		if(!head || !next || !post || !stack) return (-1);    /* check inputs */
		stack[0] = j;                 /* place j on the stack */
		while (top >= 0)                /* while (stack is not empty) */
		{
			p = stack[top];           /* p = top of stack */
			i = head[p];              /* i = youngest child of p */
			if(i == -1)
			{
				top--;                 /* p has no unordered children left */
				post[k++] = p;        /* node p is the kth postordered node */
			}
			else
			{
				head[p] = next[i];   /* remove i from children of p */
				stack[++top] = i;     /* start dfs on child node i */
			}
		}
		return k;
	}
}

/** \internal
* Approximate minimum degree ordering algorithm.
* \returns the permutation P reducing the fill-in of the input matrix \a C
* The input matrix \a C must be a selfadjoint compressed column major SparseMatrix object. Both the upper and lower parts have to be stored, but the diagonal entries are optional.
* On exit the values of C are destroyed */
template<class el_type> 
inline void lilc_matrix<el_type> :: sym_amd(vector<int>& perm) {
	using std::sqrt;
	using std::min;
	using std::max;

	int d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
	k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
	ok, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t;
	unsigned int h;

	int n = this->n_rows();
	dense = max(16, int(10 * sqrt(double(n))));   /* find dense threshold */
	dense = min(n-2, dense);
		
	perm.resize(n);

	int* W       = new int[8*(n+1)]; /* get workspace */
	int* len     = W;
	int* nv      = W +   (n+1);
	int* next    = W + 2*(n+1);
	int* head    = W + 3*(n+1);
	int* elen    = W + 4*(n+1);
	int* degree  = W + 5*(n+1);
	int* w       = W + 6*(n+1);
	int* hhead   = W + 7*(n+1);
	
	int* temp_perm = new int[n+1];
	int* last	   = temp_perm;	 //using temp_perms storage space as workspace for last

	/* --- Initialize quotient graph ---------------------------------------- */
	int cnz = this->nnz();
	t = 3*cnz + 3*cnz/5 + 2*n;                 /* add elbow room to C */
	int* Cp = new int[n+1];
	int* Ci = new int[t];
	
	int cnt = 0;
	Cp[0] = 0;
	for (int i = 0; i < n; i++) {
		for (int j = 0; j < (int) list[i].size(); j++) {
			Ci[cnt++] = list[i][j];
		}
		for (int j = 0; j < (int) m_idx[i].size(); j++) {
			Ci[cnt++] = m_idx[i][j];
		}
		Cp[i+1] = cnt;
	}
	cnz = cnt;
	
	for(k = 0; k < n; k++)
		len[k] = Cp[k+1] - Cp[k];
	len[n] = 0;
	nzmax = t;

	for(i = 0; i <= n; i++)
	{
		head[i]   = -1;                     // degree list i is empty
		last[i]   = -1;
		next[i]   = -1;
		hhead[i]  = -1;                     // hash list i is empty 
		nv[i]     = 1;                      // node i is just one node
		w[i]      = 1;                      // node i is alive
		elen[i]   = 0;                      // Ek of node i is empty
		degree[i] = len[i];                 // degree of node i
	}
	mark = amd::wclear(0, 0, w, n);         /* clear w */
	elen[n] = -2;                         /* n is a dead element */
	Cp[n] = -1;                           /* n is a root of assembly tree */
	w[n] = 0;                             /* n is a dead element */

	/* --- Initialize degree lists ------------------------------------------ */
	for(i = 0; i < n; i++)
	{
		d = degree[i];
		if(d == 0)                         /* node i is empty */
		{
			elen[i] = -2;                 /* element i is dead */
			nel++;
			Cp[i] = -1;                   /* i is a root of assembly tree */
			w[i] = 0;
		}
		else if(d > dense)                 /* node i is dense */
		{
			nv[i] = 0;                    /* absorb i into element n */
			elen[i] = -1;                 /* node i is dead */
			nel++;
			Cp[i] = amd::amd_flip (n);
			nv[n]++;
		}
		else
		{
			if(head[d] != -1) last[head[d]] = i;
			next[i] = head[d];           /* put node i in degree list d */
			head[d] = i;
		}
	}

	while (nel < n)                         /* while (selecting pivots) do */
	{
		/* --- Select node of minimum approximate degree -------------------- */
		for(k = -1; mindeg < n && (k = head[mindeg]) == -1; mindeg++) {}
		if(next[k] != -1) last[next[k]] = -1;
		head[mindeg] = next[k];          /* remove k from degree list */
		elenk = elen[k];                  /* elenk = |Ek| */
		nvk = nv[k];                      /* # of nodes k represents */
		nel += nvk;                        /* nv[k] nodes of A eliminated */
		
		/* --- Garbage collection ------------------------------------------- */
		if(elenk > 0 && cnz + mindeg >= nzmax)
		{
			for(j = 0; j < n; j++)
			{
				if((p = Cp[j]) >= 0)      /* j is a live node or element */
				{
					Cp[j] = Ci[p];          /* save first entry of object */
					Ci[p] = amd::amd_flip (j);    /* first entry is now amd::amd_flip(j) */
				}
			}
			for(q = 0, p = 0; p < cnz; ) /* scan all of memory */
			{
				if((j = amd::amd_flip (Ci[p++])) >= 0)  /* found object j */
				{
					Ci[q] = Cp[j];       /* restore first entry of object */
					Cp[j] = q++;          /* new pointer to object j */
					for(k3 = 0; k3 < len[j]-1; k3++) Ci[q++] = Ci[p++];
				}
			}
			cnz = q;                       /* Ci[cnz...nzmax-1] now free */
		}
		
		/* --- Construct new element ---------------------------------------- */
		dk = 0;
		nv[k] = -nvk;                     /* flag k as in Lk */
		p = Cp[k];
		pk1 = (elenk == 0) ? p : cnz;      /* do in place if elen[k] == 0 */
		pk2 = pk1;
		
		for(k1 = 1; k1 <= elenk + 1; k1++)
		{
			if(k1 > elenk)
			{
				e = k;                     /* search the nodes in k */
				pj = p;                    /* list of nodes starts at Ci[pj]*/
				ln = len[k] - elenk;      /* length of list of nodes in k */
			}
			else
			{
				e = Ci[p++];              /* search the nodes in e */
				pj = Cp[e];
				ln = len[e];              /* length of list of nodes in e */
			}
			for(k2 = 1; k2 <= ln; k2++)
			{
				i = Ci[pj++];
				if((nvi = nv[i]) <= 0) continue; /* node i dead, or seen */
				dk += nvi;                 /* degree[Lk] += size of node i */
				nv[i] = -nvi;             /* negate nv[i] to denote i in Lk*/
				Ci[pk2++] = i;            /* place i in Lk */
				if(next[i] != -1) last[next[i]] = last[i];
				if(last[i] != -1)         /* remove i from degree list */
				{
					next[last[i]] = next[i];
				}
				else
				{
					head[degree[i]] = next[i];
				}
			}
			if(e != k)
			{
				Cp[e] = amd::amd_flip (k);      /* absorb e into k */
				w[e] = 0;                 /* e is now a dead element */
			}
		}
		if(elenk != 0) cnz = pk2;         /* Ci[cnz...nzmax] is free */
		degree[k] = dk;                   /* external degree of k - |Lk\i| */
		Cp[k] = pk1;                      /* element k is in Ci[pk1..pk2-1] */
		len[k] = pk2 - pk1;
		elen[k] = -2;                     /* k is now an element */
		
		/* --- Find set differences ----------------------------------------- */
		mark = amd::wclear(mark, lemax, w, n);  /* clear w if necessary */
		for(pk = pk1; pk < pk2; pk++)    /* scan 1: find |Le\Lk| */
		{
			i = Ci[pk];
			if((eln = elen[i]) <= 0) continue;/* skip if elen[i] empty */
			nvi = -nv[i];                      /* nv[i] was negated */
			wnvi = mark - nvi;
			for(p = Cp[i]; p <= Cp[i] + eln - 1; p++)  /* scan Ei */
			{
				e = Ci[p];
				if(w[e] >= mark)
				{
					w[e] -= nvi;          /* decrement |Le\Lk| */
				}
				else if(w[e] != 0)        /* ensure e is a live element */
				{
					w[e] = degree[e] + wnvi; /* 1st time e seen in scan 1 */
				}
			}
		}
		
		/* --- Degree update ------------------------------------------------ */
		for(pk = pk1; pk < pk2; pk++)    /* scan2: degree update */
		{
			i = Ci[pk];                   /* consider node i in Lk */
			p1 = Cp[i];
			p2 = p1 + elen[i] - 1;
			pn = p1;
			for(h = 0, d = 0, p = p1; p <= p2; p++)    /* scan Ei */
			{
				e = Ci[p];
				if(w[e] != 0)             /* e is an unabsorbed element */
				{
					dext = w[e] - mark;   /* dext = |Le\Lk| */
					if(dext > 0)
					{
						d += dext;         /* sum up the set differences */
						Ci[pn++] = e;     /* keep e in Ei */
						h += e;            /* compute the hash of node i */
					}
					else
					{
						Cp[e] = amd::amd_flip (k);  /* aggressive absorb. e->k */
						w[e] = 0;             /* e is a dead element */
					}
				}
			}
			elen[i] = pn - p1 + 1;        /* elen[i] = |Ei| */
			p3 = pn;
			p4 = p1 + len[i];
			for(p = p2 + 1; p < p4; p++) /* prune edges in Ai */
			{
				j = Ci[p];
				if((nvj = nv[j]) <= 0) continue; /* node j dead or in Lk */
				d += nvj;                  /* degree(i) += |j| */
				Ci[pn++] = j;             /* place j in node list of i */
				h += j;                    /* compute hash for node i */
			}
			if(d == 0)                     /* check for mass elimination */
			{
				Cp[i] = amd::amd_flip (k);      /* absorb i into k */
				nvi = -nv[i];
				dk -= nvi;                 /* |Lk| -= |i| */
				nvk += nvi;                /* |k| += nv[i] */
				nel += nvi;
				nv[i] = 0;
				elen[i] = -1;             /* node i is dead */
			}
			else
			{
				degree[i] = std::min(degree[i], d);   /* update degree(i) */
				Ci[pn] = Ci[p3];         /* move first node to end */
				Ci[p3] = Ci[p1];         /* move 1st el. to end of Ei */
				Ci[p1] = k;               /* add k as 1st element in of Ei */
				len[i] = pn - p1 + 1;     /* new len of adj. list of node i */
				h %= n;                    /* finalize hash of i */
				next[i] = hhead[h];      /* place i in hash bucket */
				hhead[h] = i;
				last[i] = h;              /* save hash of i in last[i] */
			}
		}                                   /* scan2 is done */
		degree[k] = dk;                   /* finalize |Lk| */
		lemax = std::max(lemax, dk);
		mark = amd::wclear(mark+lemax, lemax, w, n);    /* clear w */
		
		/* --- Supernode detection ------------------------------------------ */
		for(pk = pk1; pk < pk2; pk++)
		{
			i = Ci[pk];
			if(nv[i] >= 0) continue;         /* skip if i is dead */
			h = last[i];                      /* scan hash bucket of node i */
			i = hhead[h];
			hhead[h] = -1;                    /* hash bucket will be empty */
			for(; i != -1 && next[i] != -1; i = next[i], mark++)
			{
				ln = len[i];
				eln = elen[i];
				for(p = Cp[i]+1; p <= Cp[i] + ln-1; p++) w[Ci[p]] = mark;
				jlast = i;
				for(j = next[i]; j != -1; ) /* compare i with all j */
				{
					ok = (len[j] == ln) && (elen[j] == eln);
					for(p = Cp[j] + 1; ok && p <= Cp[j] + ln - 1; p++)
					{
						if(w[Ci[p]] != mark) ok = 0;    /* compare i and j*/
					}
					if(ok)                     /* i and j are identical */
					{
						Cp[j] = amd::amd_flip (i);  /* absorb j into i */
						nv[i] += nv[j];
						nv[j] = 0;
						elen[j] = -1;         /* node j is dead */
						j = next[j];          /* delete j from hash bucket */
						next[jlast] = j;
					}
					else
					{
						jlast = j;             /* j and i are different */
						j = next[j];
					}
				}
			}
		}
		
		/* --- Finalize new element------------------------------------------ */
		for(p = pk1, pk = pk1; pk < pk2; pk++)   /* finalize Lk */
		{
			i = Ci[pk];
			if((nvi = -nv[i]) <= 0) continue;/* skip if i is dead */
			nv[i] = nvi;                      /* restore nv[i] */
			d = degree[i] + dk - nvi;         /* compute external degree(i) */
			d = std::min(d, n - nel - nvi);
			if(head[d] != -1) last[head[d]] = i;
			next[i] = head[d];               /* put i back in degree list */
			last[i] = -1;
			head[d] = i;
			mindeg = std::min(mindeg, d);       /* find new minimum degree */
			degree[i] = d;
			Ci[p++] = i;                      /* place i in Lk */
		}
		nv[k] = nvk;                      /* # nodes absorbed into k */
		if((len[k] = p-pk1) == 0)         /* length of adj list of element k*/
		{
			Cp[k] = -1;                   /* k is a root of the tree */
			w[k] = 0;                     /* k is now a dead element */
		}
		if(elenk != 0) cnz = p;           /* free unused space in Lk */
	}

	
	/* --- Postordering ----------------------------------------------------- */
	for(i = 0; i < n; i++) Cp[i] = amd::amd_flip (Cp[i]);/* fix assembly tree */
	for(j = 0; j <= n; j++) head[j] = -1;
	for(j = n; j >= 0; j--)              /* place unordered nodes in lists */
	{
		if(nv[j] > 0) continue;          /* skip if j is an element */
		next[j] = head[Cp[j]];          /* place j in list of its parent */
		head[Cp[j]] = j;
	}
	for(e = n; e >= 0; e--)              /* place elements in lists */
	{
		if(nv[e] <= 0) continue;         /* skip unless e is an element */
		if(Cp[e] != -1)
		{
			next[e] = head[Cp[e]];      /* place e in list of its parent */
			head[Cp[e]] = e;
		}
	}
	
	for(k = 0, i = 0; i <= n; i++)       /* postorder the assembly tree */
	{
		if(Cp[i] == -1) k = amd::tdfs(i, k, head, next, temp_perm, w);
	}

	for (int i = 0; i < n; i++) {
		perm[i] = temp_perm[i];
	}
	
	delete[] W;
	delete[] temp_perm;
	delete[] Cp;
	delete[] Ci;
	
}

#endif